# Copyright (c) 2025 lotusymt
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.
# AI-assisted: initial draft generated by an AI tool; fully reviewed and edited by the author.

import os
from typing import Any, cast
import unittest

from ament_index_python.packages import get_package_share_directory
from launch import LaunchDescription
from launch.actions import ExecuteProcess, SetEnvironmentVariable
from launch_ros.actions import Node
import launch_testing
from launch_testing.actions import ReadyToTest
import pytest


@pytest.mark.launch_test
def generate_test_description() -> tuple[LaunchDescription, dict[str, Any]]:
    # Where our package keeps test assets (bags + YAML)
    pkg_share = get_package_share_directory('nav2_collision_monitor')

    # This is the *real* rosbag we want to replay in the test
    bag_dir = os.path.join(pkg_share, 'test', 'bags', 'cm_moving_obstacle')

    # CM node parameters (must match the topics from the bag)
    params_yaml = os.path.join(
        pkg_share, 'test', 'collision_monitor_node_bag.yaml')

    # The actual gtest binary is passed in via env
    # so colcon/ctest can control which binary to run.
    test_exe = os.environ.get('TEST_EXECUTABLE')
    if not test_exe:
        # Fail early if we forgot to set it in CMake/CTest
        raise RuntimeError('TEST_EXECUTABLE env var not set')

    # Where to drop the gtest XML (so CI can read it)
    results_dir = os.environ.get('AMENT_TEST_RESULTS_DIR', '/tmp')
    xml_dir = os.path.join(results_dir, 'nav2_collision_monitor')
    os.makedirs(xml_dir, exist_ok=True)
    gtest_xml = os.path.join(
        xml_dir, 'collision_monitor_node_bag_gtest.gtest.xml')

    # Some tests set ROSCONSOLE_CONFIG_FILE → clear it to make logs visible
    clear_rosconsole = SetEnvironmentVariable(
        name='ROSCONSOLE_CONFIG_FILE', value='')

    # Bring up lifecycle manager so CM can transition automatically
    lifecycle_mgr = Node(
        package='nav2_lifecycle_manager',
        executable='lifecycle_manager',
        name='lifecycle_manager_collision_monitor',
        output='screen',
        parameters=[
            {'use_sim_time': True},     # we are using /clock from bag
            {'autostart': True},        # immediately activate CM
            {'bond_timeout': 0.0},      # do not hang test on bond
            {'node_names': ['collision_monitor']},
        ],
    )

    # The node under test
    collision_monitor = Node(
        package='nav2_collision_monitor',
        executable='collision_monitor',
        name='collision_monitor',
        output='screen',
        parameters=[params_yaml, {'use_sim_time': True}],
    )

    # Play the bag that has: /clock, /cmd_vel (or /cmd_vel_smoothed), and costmap
    bag_play = ExecuteProcess(
        cmd=['ros2', 'bag', 'play', bag_dir, '--rate', '1.0',
             '--read-ahead-queue-size', '1000'],
        output='screen',
    )

    # Run the *C++* test that actually measures “did CM stop fast enough?”
    # This runs in the same process space as launch_testing → we can assert on it later.
    cm_gtest = ExecuteProcess(
        cmd=[test_exe, f'--gtest_output=xml:{gtest_xml}'],
        output='screen',
    )

    # We return everything as a single launch description.
    # Important: ReadyToTest() tells launch_testing “ok, start the Python assertions”.
    ld = LaunchDescription([
        clear_rosconsole,
        lifecycle_mgr,
        collision_monitor,
        bag_play,
        cm_gtest,
        ReadyToTest(),  # type: ignore[no-untyped-call]
    ])
    # The dict here exposes cm_gtest to the test class below
    return ld, {'cm_gtest': cm_gtest}


@pytest.mark.launch_test
class TestWaitForGTest(unittest.TestCase):
    # This part just waits until the gtest process finished (or times out)
    def test_gtest_completed(self, proc_info: Any, cm_gtest: Any) -> None:
        proc_info.assertWaitForShutdown(process=cm_gtest, timeout=40.0)


# make the decorator explicitly untyped for mypy
post_shutdown_test = cast(Any, launch_testing.post_shutdown_test)


@pytest.mark.launch_test
@post_shutdown_test()
class TestGTestExitCode(unittest.TestCase):
    # And this part says: the gtest must have *passed*
    def test_gtest_passed(self, proc_info: Any, cm_gtest: Any) -> None:
        from launch_testing.asserts import assertExitCodes

        # assertExitCodes is also untyped, tell mypy that we know it
        assert_exit_codes = cast(Any, assertExitCodes)
        assert_exit_codes(proc_info, process=cm_gtest)
